﻿
using System;
using System.Collections.Generic;
using System.Linq;
using System.Web;
using System.Diagnostics;
using System.Runtime.Serialization;
using System.IO;
using System.Collections;
using Airport.AirportDataTypes;

namespace Airport
{
    public class SuggestionEngine : ISuggestionEngine
    {
        // Maximum number of minutes to try offsetting a Route start time
        private const int _maxRouteStartOffset = 10;
        private const bool _debugMessages = false;

        // Pointer to our service
        private AirportService _airportSVC;

        public SuggestionEngine(AirportService airportSVC)
        {
            _airportSVC = airportSVC;
        }

        public SuggestionEngine(IAirportService airportSVC)
        {
            _airportSVC = (AirportService)airportSVC;
        }
        
        public Route GetSuggestion(Plane plane, Node destinationNode, bool returnMitigation)
        {
            try
            {
                /*
                Debug.Write("plane.Location = " + plane.Location.ToString());
                foreach (Node neighbor in plane.Location.Neighbors)
                {
                    Debug.Write(neighbor.ToString());
                }
                Debug.Write("\n");
                */
                List<KeyValuePair<Node, Edge>> shortestPath;
                Route suggestionRoute = new Route();

                // Set the Suggestion's Route's Plane to plane
                suggestionRoute.RoutePlane = plane;

                if (plane == null)
                {
                    throw (new Exception("Plane parameter is null"));
                }
                if (destinationNode == null)
                {
                    throw (new Exception("DestinationNode is null"));
                }
                
                /*
                var startNodeQuery = from node in _airportSVC.AirportNodes
                                     where node == plane.Location
                                     select node;
                */
                //IEnumerable<Node> startNodeQuery;
                var startEdgeQuery = from edge in _airportSVC.AirportEdges
                                     where edge == plane.CurrentEdge
                                     select edge;
                var destinationNodeQuery = from node in _airportSVC.AirportNodes
                                           where node == destinationNode
                                           select node;

                Node airportStartNode;
                Edge airportStartEdge = startEdgeQuery.First();
                Node airportDestinationNode = destinationNodeQuery.First();

                Debug.WriteLine("GetSuggestion: airportStartEdge = {0}", airportStartEdge);
                Debug.WriteLine("GetSuggestion: airportDestinationNode = {0}", airportDestinationNode);
                
                airportStartNode = plane.CurrentEdge.OtherEndpoint(plane.PreviousMapNode);

                // If the Destination Node is the same as the Plane's current location, make shortestPath 1 Node long
                if (airportDestinationNode == plane.Location)
                {
                    shortestPath = new List<KeyValuePair<Node, Edge>>();
                    shortestPath.Add(new KeyValuePair<Node, Edge>(plane.Location, null));
                }
                else
                {
                    shortestPath = GetShortestPathViaEdges(airportStartNode, plane.PositionVector, airportStartEdge, airportDestinationNode);
                }

                if (shortestPath == null)
                {
                    throw (new Exception("Shortest Path is null"));
                }

                foreach (KeyValuePair<Node, Edge> nodeEdge in shortestPath)
                {
                    Debug.WriteLine(nodeEdge.Key.ToString());
                }

                // Set the Route's RouteEdges
                suggestionRoute.RouteEdges = (from nodeEdgePair in shortestPath
                                              where nodeEdgePair.Value != null
                                              select nodeEdgePair.Value).ToList();

                // Set RouteNode timing
                DateTime startTime = DateTime.Now;
                suggestionRoute.RouteNodes = SetRouteNodeTiming(startTime, shortestPath);

                // If we want to return a mitigated route, check for incursions and send back the safe route
                if (returnMitigation)
                {
                    Incursion detectedIncursion;
                    lock (_airportSVC._activePlanesLock)
                    {
                        detectedIncursion = CheckForIncursions(suggestionRoute);

                        if (detectedIncursion != null)
                        {
                            Debug.WriteLine("** GetSuggestion: Incursion Detected **");

                            // TODO Find a different Route/Timing
                            //suggestionRoute = SuggestMitigation(startTime, suggestionRoute);
                            Mitigation mitigation = SuggestMitigation(startTime, suggestionRoute);
                            suggestionRoute = mitigation.MitigatedRoute;
                        }
                        else
                        {
                            Debug.WriteLine("** GetSuggestion: No Incursion Detected **");
                        }
                    }
                }
                // Return the suggested Route
                return suggestionRoute;
            }
            catch (Exception ex)
            {
                Debug.WriteLine("SuggestionEngine.GetSuggestion: {0}", ex.Message);

                // Should this return an empty Suggestion?
                return null;
            }
        }

        public List<RouteNode> SetRouteNodeTiming(DateTime startTime, List<KeyValuePair<Node, Edge>> nodes)
        {
            // Turn shortestPath into a List of RouteNodes
            //DateTime currentTime = DateTime.Now;
            List<RouteNode> timedRoute = new List<RouteNode>();
            RouteNode currentRouteNode = new RouteNode(nodes[0].Key);
            currentRouteNode.PredictedTOA = startTime;
            timedRoute.Add(currentRouteNode);

            for (int i = 1; i < nodes.Count; i++)
            {
                DateTime previousTOA = currentRouteNode.PredictedTOA;

                currentRouteNode = new RouteNode(nodes[i].Key);
                Edge previousEdge = nodes[i - 1].Value;

                // Truncate travel time because it's better to predict early arrival at a node than
                // to predict late arrival
                int totalTravelTimeInSeconds = (int)(previousEdge.Length / previousEdge.AverageSpeed);

                int travelTimeSeconds = totalTravelTimeInSeconds % 60;
                int travelTimeMinutes = (int)((totalTravelTimeInSeconds / 60) % 60);
                int travelTimeHours = (int)(totalTravelTimeInSeconds / 3600);

                TimeSpan timeToNextNode = new TimeSpan(travelTimeHours, travelTimeMinutes, travelTimeSeconds);
                currentRouteNode.PredictedTOA = previousTOA.Add(timeToNextNode);

                timedRoute.Add(currentRouteNode);
            }

            return timedRoute;
        }

        /// <summary>
        /// Check if the given Route causes an incursion with an existing Route.
        /// </summary>
        /// <param name="pendingRoute"></param>
        /// <returns>An Incursion object describing the detected incursion. null otherwise.</returns>
        public Incursion CheckForIncursions(Route pendingRoute)
        {
            List<Route> activeRoutesList;
            
            activeRoutesList = (from activePlane in _airportSVC.ActivePlanes
                                where activePlane != pendingRoute.RoutePlane 
                                    && activePlane.CurrentRoute != null 
                                    && !pendingRoute.Equals(activePlane.CurrentRoute)
                                select activePlane.CurrentRoute).ToList();
                
            /*
            Incursion foundIncursion = (from route in activeRoutesList
                                        from activeRouteNode in route.RouteNodes
                                         from pendingRouteNode in pendingRoute.RouteNodes
                                         //let overlapIndex = route.RouteNodes.BinarySearch(pendingRouteNode, new PredictedTOACompare())
                                         //where (overlapIndex >= 0)
                                         //   && ((Node)pendingRouteNode).Equals((Node)route.RouteNodes[overlapIndex])
                                         where //((Node)pendingRouteNode).Equals((Node)activeRouteNode)
                                             pendingRouteNode.Equals(activeRouteNode)
                                         select new Incursion(-1, 
                                             new Plane[] { pendingRoute.RoutePlane, route.RoutePlane }, 
                                             pendingRouteNode, 
                                             new Route[] { route, pendingRoute })).FirstOrDefault();
            */
            Incursion foundIncursion = null;
            foreach (Route route in activeRoutesList)
            {
                foreach (RouteNode activeRouteNode in route.RouteNodes)
                {
                    int numPendingRouteNodes = pendingRoute.RouteNodes.Count;
                    //foreach (RouteNode pendingRouteNode in pendingRoute.RouteNodes)
                    for (int i = 0; i < numPendingRouteNodes; i++)
                    {
                        //Debug.WriteLine(" ** SuggestionEngine.CheckForIncursion **");
                        /*
                        if (!((i == numPendingRouteNodes - 1) && pendingRoute.RouteNodes[i].IsHoldBar)
                            && pendingRoute.RouteNodes[i].Equals(activeRouteNode))
                        */
                        if (pendingRoute.RouteNodes[i].Equals(activeRouteNode))
                        {
                            //Debug.WriteLine(" ** SuggestionEngine.CheckForIncursion Found Equal RouteNode **");
                            foundIncursion = new Incursion(-1,
                                                new Plane[] { pendingRoute.RoutePlane, route.RoutePlane },
                                                pendingRoute.RouteNodes[i],
                                                new Route[] { route, pendingRoute });
                            break;
                        }
                    }
                    if (foundIncursion != null)
                    {
                        break;
                    }
                }
                
                if (foundIncursion != null)
                {
                    break;
                }
            }

            if (foundIncursion != null)
            {
                string incursionIntersection = "";
                List<Edge> intersectingEdges = new List<Edge>();

                // Find the names of all intersecting Edges
                foreach (Route route in foundIncursion.IncursionRoutes)
                {
                    Edge intersectingEdge = (from edge in route.RouteEdges
                                             where edge.Endpoint1 == (Node)foundIncursion.IncursionRouteNode 
                                                || edge.Endpoint2 == (Node)foundIncursion.IncursionRouteNode
                                             select edge).FirstOrDefault();

                    incursionIntersection += intersectingEdge.Name + ", ";
                }

                foundIncursion.Intersection = incursionIntersection;
                Debug.WriteLine("** CheckForIncursions: Incursion Found **");
            }
            else
            {
                Debug.WriteLine("** CheckForIncursions: No Incursions Found **");
            }

            return foundIncursion;
        }

        //public Route SuggestMitigation(DateTime originalStartTime, List<KeyValuePair<Node, Edge>> unsafeRoute)
        //public Route SuggestMitigation(DateTime originalStartTime, Route unsafeRoute)
        public Mitigation SuggestMitigation(DateTime originalStartTime, Route unsafeRoute)
        {
            List<Route> offsetStartRoutes = new List<Route>();
            Route pendingOffsetRoute = new Route();
            
            // Get Actual Plane Instance
            Plane actualPlane = (from plane in _airportSVC.ActivePlanes
                                 where unsafeRoute.RoutePlane.Equals(plane)
                                 select plane).FirstOrDefault();

            //unsafeRoute.RoutePlane.Status = actualPlane.Status;

            // If the Plane has been cleared for Takeoff, Suggest Cancel Takeoff
            if (actualPlane.Status == PlaneStatus.ClearedForTakeoff
                || actualPlane.Status == PlaneStatus.TakingOff)
            {
                Debug.WriteLine(" ** SuggestMitigation: Cancel Takeoff! **");
                Route emptyRoute = new Route();
                emptyRoute.RoutePlane = unsafeRoute.RoutePlane;
                
                return new CancelTakeoffMitigation(actualPlane);
            }

            // Try offsetting start time up to _maxRouteStartOffset minutes
            for (int i = 1; i <= _maxRouteStartOffset; i++)
            {
                TimeSpan offset = new TimeSpan(0, i, 0);
                pendingOffsetRoute.RoutePlane = unsafeRoute.RoutePlane;
                pendingOffsetRoute.RouteNodeEdgePairs = unsafeRoute.RouteNodeEdgePairs;
                pendingOffsetRoute.RouteNodes = SetRouteNodeTiming(originalStartTime + offset, unsafeRoute.NodeEdgePairs);
                pendingOffsetRoute.StartTime = originalStartTime + offset;

                if (CheckForIncursions(pendingOffsetRoute) == null)
                {
                    offsetStartRoutes.Add(pendingOffsetRoute);
                }
            }

            // Try adding hold instruction
            // Start at the second route node because holding at
            // the first route node is equivalent to offsetting
            // the start time
            List<Route> holdRoutes = new List<Route>();
            
            for (int j = 1; j < unsafeRoute.RouteNodes.Count(); j++)
            {
                // If this route node is a designated hold bar
                if (unsafeRoute.RouteNodes[j].IsHoldBar)
                {
                    Debug.WriteLine("* SuggestMitigation: Found Hold Bar *");
                    Route pendingHoldRoute = new Route(unsafeRoute);
                    pendingHoldRoute.RoutePlane = unsafeRoute.RoutePlane;
                    //pendingHoldRoute.RoutePlane = unsafeRoute.RoutePlane;
                    // Get the route up to the hold bar
                    pendingHoldRoute.RouteNodeEdgePairs = unsafeRoute.RouteNodeEdgePairs.GetRange(0, j + 1);

                    if (CheckForIncursions(pendingHoldRoute) == null)
                    {
                        Debug.WriteLine("* SuggestMitigation: Added Hold Bar *");
                        holdRoutes.Add(pendingHoldRoute);
                    }
                }
            }
            
            string mitigatedRoutes = "** SuggestMitigation: **\n";
            foreach (Route offset in offsetStartRoutes)
            {
                mitigatedRoutes += "offsetStartRoutes: " + offset.ToString() + "\n";
            }
            foreach (Route offset in holdRoutes)
            {
                mitigatedRoutes += "holdRoutes: " + offset.ToString() + "\n";
            }
            //Debug.WriteLine(mitigatedRoutes);

            Route mitigatedRoute;
            // Find the most efficient mitigation
            // Prefer adding a hold instruction because it keeps 
            // traffic moving
            if (holdRoutes.Count > 0)
            {
                // Prefer holding later in the route
                //return holdRoutes.Last();
                mitigatedRoute = holdRoutes.Last();
                Node holdNode = mitigatedRoute.RouteNodes.Last();
                Edge lastEdge = mitigatedRoute.RouteEdges.Last();
                HoldMitigation holdMitigation = new HoldMitigation(unsafeRoute.RoutePlane, mitigatedRoute);
                //HoldMitigation holdMitigation = new HoldMitigation(actualPlane);
                
                // Find the other edges that intersect at this point
                /*
                string intersectingEdgeName = (from edge in _airportSVC.AirportEdges
                                                where (edge.Endpoint1.Equals(holdNode)
                                                        || edge.Endpoint2.Equals(holdNode))
                                                select edge.Name).FirstOrDefault();
                */
                // Find the name of the surface of which the plane should hold short
                string holdShortName = (from edge in _airportSVC.AirportEdges
                                        where lastEdge.Equals(edge)
                                        select edge.HoldingShortOfSurfaceName).FirstOrDefault();

                holdMitigation.HoldIntersection = holdShortName;
                
                return holdMitigation;
                //return unsafeRoute.RoutePlane.Name + " Hold at " + intersectingEdgeName;
            }
            else
            {
                // Prefer starting earlier
                //return offsetStartRoutes.First();
                mitigatedRoute = offsetStartRoutes.First();
                DelayStartMitigation delayStartMitigation = new DelayStartMitigation(unsafeRoute.RoutePlane, mitigatedRoute);
                //DelayStartMitigation delayStartMitigation = new DelayStartMitigation(actualPlane, offsetStartRoutes.First().StartTime);

                return delayStartMitigation;
                //return unsafeRoute.RoutePlane.Name + " Delay Start Until " + offsetStartRoutes.First().StartTime.ToString("HH:mm:ss");
            }
        }

        public Route GetPathFromStrings(List<string> routeStrings, bool overrideAccessibleEdges)
        {
            Route finalRoute = new Route();
            List<KeyValuePair<Node, Edge>> workingRoute = new List<KeyValuePair<Node, Edge>>();

            // TODO Does this need to be 3?
            if (routeStrings.Count < 2)
            {
                return null;
            }

            string planeName = routeStrings[0];
            Plane foundPlane = null;

            lock (_airportSVC._activePlanesLock)
            {
                // Find the plane that has either the same PlaneName or FlightName
                foundPlane = (from plane in _airportSVC.ActivePlanes
                                 where (plane.Name.Equals(planeName, StringComparison.CurrentCultureIgnoreCase))
                                            || ((plane.PlaneFlight != null) && (plane.PlaneFlight.FlightName.Equals(planeName, StringComparison.CurrentCultureIgnoreCase)))
                                 select plane).FirstOrDefault();
            }
            /*
            int planeCount = planeQuery.Count();

            if (planeCount == 1)
            {
                foundPlane = planeQuery.First();
            }
            else if (planeCount > 1)
            {
                Debug.WriteLine("Error: SuggestionEngine.GetPathFromStrings: More than 1 plane or flight with this name found.");
            }
            else
            {
                Debug.WriteLine("Error: SuggestionEngine.GetPathFromStrings: No plane or flight with this name found.");
            }
            */
            if (foundPlane == null)
            {
                Debug.WriteLine("Plane not found.");
                return null;
            }

            // Set the Plane's current position vector as the final Route's start point
            finalRoute.StartPoint = new Point((int)foundPlane.PositionVector.X, (int)foundPlane.PositionVector.Y);

            var startEdgeQuery = from edge in _airportSVC.AirportEdges
                                 where edge == foundPlane.CurrentEdge
                                 select edge;
            
            // Get the actual reference in AirportEdges of the Plane's CurrentEdge
            Edge startingEdge = startEdgeQuery.First();

            // Arbitrarily choose Endpoint1 to be the Node that partitions the current surface
            //Node middleNode = startingEdge.Endpoint1;

            // The plane should be starting on the first edge of the Route
            if (!routeStrings[1].Equals(startingEdge.Name, StringComparison.CurrentCultureIgnoreCase))
            {
                Debug.WriteLine("Plane not located on the first Edge of the Route.");
                return null;
            }

            // List of the edges of all the surfaces of the route
            List<List<Edge>> routeSurfaces = new List<List<Edge>>();

            // Iterate over route strings
            for (int i = 1; i < routeStrings.Count - 1; i++)
            {
                // Get all the Edges comprising the current taxiway/runway
                var surfaceEdges = from edge in _airportSVC.AirportEdges
                                   where edge.Name.Equals(routeStrings[i])
                                   select edge;

                List<Edge> surfaceEdgesList = surfaceEdges.ToList();

                routeSurfaces.Add(surfaceEdgesList);
            }

            Node lastNode = null; 

            for (int j = 0; j < routeSurfaces.Count; j++)
            {
                //List<Edge> overlapWithNextSurfaceList;
                bool foundPreviousSurface = false;
                bool foundNextSurface = false;
                int closestEdgeToNextSurfaceIndex = -1;
                int closestEdgeToPreviousSurfaceIndex = -1;

                // Find the edges of the current surface that overlap with the next surface
                /*
                var overlapWithNextSurface = from edge in routeSurfaces[j]
                                             where edge.AccessibleNeighboringEdges[edge.Endpoint1].Any(e => e.Name.Equals(routeStrings[j + 2]))
                                                    || edge.AccessibleNeighboringEdges[edge.Endpoint2].Any(e => e.Name.Equals(routeStrings[j + 2]))
                                             select edge;
                */
                //overlapWithNextSurfaceList = overlapWithNextSurface.ToList();
                Node pathLegStartNode = null;
                Node pathLegEndNode = null;
                for (int k = 0; k < routeSurfaces[j].Count; k++)
                {
                    Edge edge = routeSurfaces[j][k];

                    if (j > 0)
                    {
                        // If this Edge intersects with the previous surface
                        if (edge.NeighboringEdges[edge.Endpoint1].Any(e => e.Name.Equals(routeStrings[j]))
                            || edge.NeighboringEdges[edge.Endpoint2].Any(e => e.Name.Equals(routeStrings[j])))
                        {
                            if (edge.NeighboringEdges[edge.Endpoint1].Any(e => e.Name.Equals(routeStrings[j])))
                            {
                                pathLegStartNode = edge.Endpoint1;
                            }
                            else
                            {
                                pathLegStartNode = edge.Endpoint2;
                            }

                            if (!foundNextSurface || !foundPreviousSurface)
                            {
                                foundPreviousSurface = true;
                                closestEdgeToPreviousSurfaceIndex = k;
                            }
                        }
                        // If this Edge intersects with the next surface
                        if (edge.NeighboringEdges[edge.Endpoint1].Any(e => e.Name.Equals(routeStrings[j + 2]))
                            || edge.NeighboringEdges[edge.Endpoint2].Any(e => e.Name.Equals(routeStrings[j + 2])))
                        {
                            if (edge.NeighboringEdges[edge.Endpoint1].Any(e => e.Name.Equals(routeStrings[j + 2])))
                            {
                                pathLegEndNode = edge.Endpoint1;
                            }
                            else
                            {
                                pathLegEndNode = edge.Endpoint2;
                            }

                            if (!foundPreviousSurface || !foundNextSurface)
                            {
                                foundNextSurface = true;
                                closestEdgeToNextSurfaceIndex = k;
                            }
                        }
                    }
                    else // First surface of Route, previous Edge should be Plane's CurrentEdge
                    {
                        if (edge.NeighboringEdges[edge.Endpoint1].Any(e => e.Name.Equals(routeStrings[j + 2])))
                        {
                            pathLegEndNode = edge.Endpoint1;
                            lastNode = edge.Endpoint2;
                            closestEdgeToNextSurfaceIndex = k;
                        }
                        else if (edge.NeighboringEdges[edge.Endpoint2].Any(e => e.Name.Equals(routeStrings[j + 2])))
                        {
                            pathLegEndNode = edge.Endpoint2;
                            lastNode = edge.Endpoint1;
                            closestEdgeToNextSurfaceIndex = k;
                        }

                        if (edge == foundPlane.CurrentEdge)
                        {
                            foundPreviousSurface = true;
                            closestEdgeToPreviousSurfaceIndex = k;

                            //if (routeSurfaces[j].Count == 1)
                            if (edge.NeighboringEdges[edge.Endpoint1].Any(e => e.Name.Equals(routeStrings[j + 2]))
                                || edge.NeighboringEdges[edge.Endpoint2].Any(e => e.Name.Equals(routeStrings[j + 2])))
                            {
                                //closestEdgeToNextSurfaceIndex = k;

                                pathLegStartNode = edge.OtherEndpoint(pathLegEndNode);
                            }
                            else
                            {
                                // Set pathLegStartNode to the Endpoint farthest from the next surface
                                //pathLegStartNode = edge.OtherEndpoint(lastNode);
                                pathLegStartNode = foundPlane.PreviousMapNode;
                            }
                        }
                        else
                        {
                            lastNode = edge.OtherEndpoint(lastNode);
                        }

                    }
                }

                //List<Edge> nextRoutePath = new List<Edge>();
                Node lastRoutePathStartNode = pathLegStartNode;
                if (closestEdgeToPreviousSurfaceIndex < closestEdgeToNextSurfaceIndex)
                {
                    for (int m = closestEdgeToPreviousSurfaceIndex; m <= closestEdgeToNextSurfaceIndex; m++)
                    {
                        //nextRoutePath.Add(routeSurfaces[j][m]);

                        workingRoute.Add(new KeyValuePair<Node, Edge>(lastRoutePathStartNode, routeSurfaces[j][m]));

                        lastRoutePathStartNode = routeSurfaces[j][m].OtherEndpoint(lastRoutePathStartNode);
                    }
                }
                else
                {
                    for (int m = closestEdgeToPreviousSurfaceIndex; m >= closestEdgeToNextSurfaceIndex; m--)
                    {
                        //nextRoutePath.Add(routeSurfaces[j][m]);

                        workingRoute.Add(new KeyValuePair<Node, Edge>(lastRoutePathStartNode, routeSurfaces[j][m]));

                        lastRoutePathStartNode = routeSurfaces[j][m].OtherEndpoint(lastRoutePathStartNode);
                    }
                }

                // Add the last Node to the route
                if (j == routeSurfaces.Count - 1)
                {
                    workingRoute.Add(new KeyValuePair<Node, Edge>(lastRoutePathStartNode, null));
                }
            }

            finalRoute.RoutePlane = foundPlane;

            // Set the Route's RouteEdges
            finalRoute.RouteEdges = (from nodeEdgePair in workingRoute
                                          where nodeEdgePair.Value != null
                                          select nodeEdgePair.Value).ToList();

            // Set RouteNode timing
            DateTime startTime = DateTime.Now;
            finalRoute.RouteNodes = SetRouteNodeTiming(startTime, workingRoute);

            return finalRoute;
        }

        #region EdgeBasedSuggestions

        public List<KeyValuePair<Node, Edge>> GetShortestPathViaEdges(Node startNode, WorldVector startPositionVector, Edge startEdge, Node endNode)
        {
            // NOTE: startNode must be one of the Endpoints of startEdge
            try
            {
                // Open List of Nodes
                Dictionary<Node, Edge> openList = new Dictionary<Node, Edge>();
                
                // Closed List of Nodes
                List<Node> closedList = new List<Node>();
                
                // Map of navigated Nodes
                Dictionary<Node, KeyValuePair<Node, Edge>> cameFrom = new Dictionary<Node, KeyValuePair<Node, Edge>>();

                // Distance from start along optimal path
                Dictionary<Node, double> gScore = new Dictionary<Node, double>();
                Dictionary<Node, double> hScore = new Dictionary<Node, double>();
                // Estimated total distance from start to end through a accessibleNeighbor
                Dictionary<Node, double> fScore = new Dictionary<Node, double>();

                openList.Add(startNode, startEdge);
                cameFrom[startNode] = new KeyValuePair<Node, Edge>(startEdge.OtherEndpoint(startNode), startEdge);
                closedList.Add(startEdge.OtherEndpoint(startNode));

                gScore[startEdge.OtherEndpoint(startNode)] = 0;
                hScore[startEdge.OtherEndpoint(startNode)] = Heuristic(startPositionVector, endNode);
                fScore[startEdge.OtherEndpoint(startNode)] = hScore[startEdge.OtherEndpoint(startNode)];

                gScore[startNode] = Heuristic(startPositionVector, startNode);
                hScore[startNode] = Heuristic(startNode, endNode);
                fScore[startNode] = gScore[startNode] + hScore[startNode];

                Node currentNode;
                Edge currentEdge;

                while (openList.Count > 0)
                {
                    // Find the node(s) with the lowest f score via Linq query
                    var lowestFNode = openList.Aggregate((l, r) => fScore[l.Key] < fScore[r.Key] ? l : r).Key;

                    // Set currentNode to the node in openList with the lowest fScore
                    currentNode = lowestFNode;
                    currentEdge = openList[currentNode];

                    if (currentNode == endNode)
                    {
                        Debug.WriteLine("Path Found!");
                        return ReconstructPathViaEdges(cameFrom, new KeyValuePair<Node, Edge>(endNode, null));
                    }

                    openList.Remove(currentNode);

                    closedList.Add(currentNode);
                    
                    foreach (Edge neighborEdge in currentEdge.AccessibleNeighboringEdges[currentNode])
                    {
                        //Debug.WriteLineIf(neighborEdge.OtherEndpoint(currentNode) == endNode, "neighborEdge: " + neighborEdge.ToString());
                        bool tentativeIsBetter = false;

                        if (closedList.Contains(neighborEdge.OtherEndpoint(currentNode)))
                        {
                            //Debug.WriteLine("closedList.Contains(neighborEdge.OtherEndpoint({0}))", currentNode);
                            continue;
                        }

                        double tentativeGScore = gScore[currentNode] + Heuristic(currentNode, neighborEdge.OtherEndpoint(currentNode));

                        if (!openList.ContainsKey(neighborEdge.OtherEndpoint(currentNode)))
                        {
                            //Debug.WriteLine("!openList.ContainsKey(neighborEdge.OtherEndpoint({0})); OtherEndpoint = {1}", currentNode, neighborEdge.OtherEndpoint(currentNode));                            
                            openList.Add(neighborEdge.OtherEndpoint(currentNode), neighborEdge);
                            tentativeIsBetter = true;
                        }
                        else if (tentativeGScore < gScore[neighborEdge.OtherEndpoint(currentNode)])
                        {
                            //Debug.WriteLine("tentativeGScore < gScore[neighborEdge.OtherEndpoint({0})]", currentNode);
                            tentativeIsBetter = true;
                        }
                        else
                        {
                            //Debug.WriteLine("else; currentNode = {0}", currentNode);
                            tentativeIsBetter = false;
                        }

                        if (tentativeIsBetter == true)
                        {
                            //Debug.WriteLine("tentativeIsBetter == true; currentNode = {0}", currentNode);
                            //Debug.WriteLineIf(neighborEdge.OtherEndpoint(currentNode) == endNode, "Neighbor: " + currentNode.ToString());
                            cameFrom[neighborEdge.OtherEndpoint(currentNode)] = new KeyValuePair<Node,Edge>(currentNode, neighborEdge);                            

                            gScore[neighborEdge.OtherEndpoint(currentNode)] = tentativeGScore;                           
                            hScore[neighborEdge.OtherEndpoint(currentNode)] =
                                    Heuristic(neighborEdge.OtherEndpoint(currentNode), endNode);
                            fScore[neighborEdge.OtherEndpoint(currentNode)] = 
                                    gScore[neighborEdge.OtherEndpoint(currentNode)] + hScore[neighborEdge.OtherEndpoint(currentNode)];
                        }
                    }                    
                }

                Debug.WriteLine("Path Not Found!");
                // If we failed to find the shortest path
                return null;
            }
            catch (Exception ex)
            {
                Debug.WriteLine("SuggestionEngine.GetShortestPath: " + ex.Message);
                return null;
            }
        }

        private List<KeyValuePair<Node, Edge>> ReconstructPathViaEdges(Dictionary<Node, KeyValuePair<Node, Edge>> cameFrom,
                    KeyValuePair<Node, Edge> destinationNodeEdge)
        {
            List<KeyValuePair<Node, Edge>> path = new List<KeyValuePair<Node, Edge>>(); ;
            KeyValuePair<Node, Edge> currentNodeEdge = destinationNodeEdge;

            Debug.WriteLine("cameFrom.Count = " + cameFrom.Count);

            while (cameFrom.ContainsKey(currentNodeEdge.Key))
            {
                path.Add(currentNodeEdge);

                currentNodeEdge = cameFrom[currentNodeEdge.Key];
            }

            path.Add(currentNodeEdge);

            // Non-recursive method returns path in reverse
            path.Reverse();

            Debug.WriteLine("Path Reconstructed!");
            return path;
        }

        private List<KeyValuePair<Node, Edge>> ReconstructPathViaEdgesRecursive(Dictionary<Node, KeyValuePair<Node, Edge>> cameFrom, 
                    KeyValuePair<Node, Edge> currentNodeEdge)
        {            
            List<KeyValuePair<Node, Edge>> path;

            if (cameFrom.ContainsKey(currentNodeEdge.Key))
            {
                Node endpoint1 = cameFrom[currentNodeEdge.Key].Value.Endpoint1;
                Node endpoint2 = cameFrom[currentNodeEdge.Key].Value.Endpoint2;

                #region Debug Print Statements
                Debug.WriteIf(_debugMessages, "currentNode: " + currentNodeEdge.ToString() + " ");
                Debug.WriteIf(_debugMessages, "cameFrom: " + cameFrom[currentNodeEdge.Key].Value.ToString() + " ");
                Debug.WriteIf(_debugMessages, "Endpoint1 Neighbors: ");
                foreach (Edge neighbor in cameFrom[currentNodeEdge.Key].Value.AccessibleNeighboringEdges[endpoint1])
                {
                    Debug.WriteIf(_debugMessages, neighbor + " " + neighbor.Endpoint1 + neighbor.Endpoint2 + " ");
                }
                Debug.WriteIf(_debugMessages, "Endpoint2 Neighbors: ");
                foreach (Edge neighbor in cameFrom[currentNodeEdge.Key].Value.AccessibleNeighboringEdges[endpoint2])
                {
                    Debug.WriteIf(_debugMessages, neighbor + " " + neighbor.Endpoint1 + neighbor.Endpoint2 + " ");
                }
                Debug.WriteIf(_debugMessages, "\n");
                #endregion

                path = ReconstructPathViaEdgesRecursive(cameFrom, cameFrom[currentNodeEdge.Key]);
                path.Add(currentNodeEdge);

                return path;
            }
            else
            {
                path = new List<KeyValuePair<Node, Edge>>();
                path.Add(currentNodeEdge);

                return path;
            }
        }

        #endregion

        #region NodeBasedSuggestions

        public Suggestion GetSuggestionViaNodes(Plane plane, Node destinationNode)
        {
            try
            {
                Debug.Write("plane.Location = " + plane.Location.ToString());
                foreach (Node neighbor in plane.Location.Neighbors)
                {
                    Debug.Write(neighbor.ToString());
                }
                Debug.Write("\n");

                Suggestion suggestion;
                List<Node> shortestPath;
                Route suggestionRoute = new Route();

                if (plane == null)
                {
                    throw (new Exception("Plane parameter is null"));
                }
                if (destinationNode == null)
                {
                    throw (new Exception("DestinationNode is null"));
                }
                /*
                start = plane.Location;
                startEdge = plane.CurrentEdge;
                int startIndex = _airportSVC.AirportNodes.IndexOf(start);
                int startEdgeIndex = _airportSVC.AirportEdges.IndexOf(startEdge);
                int destinationIndex = _airportSVC.AirportNodes.IndexOf(destinationNode);
                */
                var startNodeQuery = from node in _airportSVC.AirportNodes
                                     where node == plane.Location
                                     select node;
                var destinationNodeQuery = from node in _airportSVC.AirportNodes
                                           where node == destinationNode
                                           select node;

                Node airportStartNode = startNodeQuery.First();
                Node airportDestinationNode = destinationNodeQuery.First();

                //shortestPath = GetShortestPath(start, destinationNode);
                shortestPath = GetShortestPath(airportStartNode, airportDestinationNode);
                //shortestPath = GetShortestPathViaEdges(_airportSVC.AirportNodes[startIndex], 
                //        _airportSVC.AirportEdges[startEdgeIndex], _airportSVC.AirportNodes[destinationIndex]);
                //shortestPath = GetShortestPathViaEdges(airportStartNode,
                //        airportStartEdge,airportDestinationNode);

                foreach (Node node in shortestPath)
                {
                    Debug.WriteLine(node.ToString());
                }

                if (shortestPath == null)
                {
                    throw (new Exception("Shortest Path is null"));
                }

                // Turn shortestPath into a List of RouteNodes
                foreach (Node node in shortestPath)
                {
                    suggestionRoute.RouteNodes.Add(new RouteNode(node));
                }

                suggestion = new Suggestion(-1, plane, suggestionRoute);

                Debug.WriteLine(suggestion);

                // returns a suggestion
                return suggestion;
            }
            catch (Exception ex)
            {
                Debug.WriteLine("SuggestionEngine.GetSuggestion: {0}", ex.Message);

                // Should this return an empty Suggestion?
                return null;
            }
        }

        public List<Node> GetShortestPath(Node start, Node end)
        {
            try
            {
                // Open List of Nodes
                List<Node> openList = new List<Node>();
                // Closed List of Nodes
                List<Node> closedList = new List<Node>();
                // Map of navigated Nodes
                Dictionary<Node, Node> cameFrom = new Dictionary<Node, Node>();
                // Distance from start along optimal path
                Dictionary<Node, double> gScore = new Dictionary<Node, double>();
                Dictionary<Node, double> hScore = new Dictionary<Node, double>();
                // Estimated total distance from start to end through a accessibleNeighbor
                Dictionary<Node, double> fScore = new Dictionary<Node, double>();

                // Add Starting Node to openList
                openList.Add(start);

                gScore[start] = 0;
                hScore[start] = Heuristic(start, end);
                fScore[start] = hScore[start];

                Node x;

                while (openList.Count > 0)
                {
                    // Find the node(s) with the lowest f score via Linq query
                    var lowestF = from node in openList
                                  where fScore[node] == openList.Min(minFScore => fScore[minFScore])
                                  select node;
                    // Set x to the node in openList with the lowest fScore
                    x = lowestF.First();

                    if (x == end)
                    {
                        return ReconstructPath(cameFrom, end);
                    }

                    openList.Remove(x);

                    if (openList.Contains(x))
                    {
                        throw (new Exception("Failed to remove node x from openList"));
                    }

                    closedList.Add(x);

                    if (!closedList.Contains(x))
                    {
                        throw (new Exception("Failed to add node x to closedList"));
                    }

                    foreach (Node neighbor in x.Neighbors)
                    {
                        //Debug.WriteLine("Neightbor: " + accessibleNeighbor.ToString());

                        bool tentativeIsBetter = false;

                        if (closedList.Contains(neighbor))
                        {
                            continue;
                        }

                        double tentativeGScore = gScore[x] + Heuristic(x, neighbor);

                        if (!openList.Contains(neighbor))
                        {
                            openList.Add(neighbor);
                            tentativeIsBetter = true;
                        }
                        else if (tentativeGScore < gScore[neighbor])
                        {
                            tentativeIsBetter = true;
                        }
                        else
                        {
                            tentativeIsBetter = false;
                        }

                        if (tentativeIsBetter == true)
                        {
                            //Debug.WriteLineIf(accessibleNeighbor == end, "Neighbor: " + x.ToString());

                            cameFrom[neighbor] = x;

                            gScore[neighbor] = tentativeGScore;
                            hScore[neighbor] = Heuristic(neighbor, end);
                            fScore[neighbor] = gScore[neighbor] + hScore[neighbor];
                        }
                    }
                    /*
                    Debug.Write("closedList: ");
                    foreach (Node node in closedList)
                    {
                        Debug.Write(node);
                    }
                    Debug.Write("\n");
                    */
                }

                // If we failed to find the shortest path
                return null;
            }
            catch (Exception ex)
            {
                Debug.WriteLine("SuggestionEngine.GetShortestPath: {0}", ex.Message);
                return null;
            }
        }

        private List<Node> ReconstructPath(Dictionary<Node, Node> cameFrom, Node currentNode)
        {
            List<Node> nodeList;
            if (cameFrom.ContainsKey(currentNode))
            {
                nodeList = ReconstructPath(cameFrom, cameFrom[currentNode]);
                nodeList.Add(currentNode);

                return nodeList;
            }
            else
            {
                nodeList = new List<Node>();
                nodeList.Add(currentNode);

                return nodeList;
            }
        }

        #endregion

        /// <summary>
        /// Heuristic method to determine the path cost between 2 nodes.
        /// </summary>
        /// <param name="start"></param>
        /// <param name="end"></param>
        /// <returns></returns>
        public double Heuristic(Node start, Node end)
        {
            return start.DistanceTo(end);
        }

        /// <summary>
        /// Heuristic method to determine the path cost between a WorldVector and a Node.
        /// </summary>
        /// <param name="start"></param>
        /// <param name="end"></param>
        /// <returns></returns>
        public double Heuristic(WorldVector startPositionVector, Node end)
        {
            return startPositionVector.DistanceToNode(end);
        } 
    }
}